// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectPoseUpdaterTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Internal.Robotics.Tools
{
    using System;

    using Microsoft.Robotics.Navigation.Runtime.Perception;
    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    /// <summary>
    /// Test KinectPoseUpdater.
    /// </summary>
    [TestClass]
    public class KinectPoseUpdaterTests
    {
        /// <summary>
        /// Test Kinect pose updates due to panning and tilting.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [Ignore]
        public void TestKinectPoseUpdate()
        {
            Pose kinect = new Pose(new Vector3(-0.059, 0, 0.539), new Quaternion(0, 0, 0, 1));

            // no tilt/pan
            Assert.IsTrue(
                this.Compare(
                kinect, KinectPoseUpdater.UpdatePose(kinect, 0, 0)));

            // Pi / 4 radian tilt up (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.085038, 0, 0.60186), new Quaternion(0, -0.38268, 0, 0.923879)), 
                    KinectPoseUpdater.UpdatePose(kinect, -MathConstants.PIOverFour, 0)));

            // Pi / 4 radian tilt down (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.085038, 0, 0.476138), new Quaternion(0, 0.382683, 0, 0.923879)),
                    KinectPoseUpdater.UpdatePose(kinect, MathConstants.PIOverFour, 0)));

            // 1 radian pan left (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.099867, -0.07480, 0.539), new Quaternion(0, 0, -0.4794, 0.8776)),
                    KinectPoseUpdater.UpdatePose(kinect, 0, -1)));

            // 1 radian pan right (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.099867, 0.07480, 0.539), new Quaternion(0, 0, 0.4794, 0.8776)),
                    KinectPoseUpdater.UpdatePose(kinect, 0, 1)));

            // Pi / 2 radian pan left (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.1479, -0.0889, 0.539), new Quaternion(0, 0, -0.7071067, 0.707106)),
                    KinectPoseUpdater.UpdatePose(kinect, 0, -MathConstants.PIOverTwo)));

            // Pi / 2 radian pan right (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.1479, 0.0889, 0.539), new Quaternion(0, 0, 0.7071067, 0.707106)),
                    KinectPoseUpdater.UpdatePose(kinect, 0, MathConstants.PIOverTwo)));

            // Pi / 4 radian pan right and tilt up (around the turret center)
            Assert.IsTrue(
                this.Compare(
                    new Pose(new Vector3(-0.10345, 0.06286, 0.5834), new Quaternion(-0.14644, -0.35355, 0.35355, 0.85355)),
                    KinectPoseUpdater.UpdatePose(kinect, -MathConstants.PIOverFour, MathConstants.PIOverFour)));
        }

        /// <summary>
        /// Compare doubles within an epsilon.
        /// </summary>
        /// <param name="a">Value 'a'.</param>
        /// <param name="b">Value 'b'.</param>
        /// <returns>Whether equivalent.</returns>
        private bool Compare(double a, double b)
        {
            return Math.Abs(a - b) < 0.0001;
        }

        /// <summary>
        /// Compare doubles within an epsilon.
        /// </summary>
        /// <param name="a">Value 'a'.</param>
        /// <param name="b">Value 'b'.</param>
        /// <returns>Whether equivalent.</returns>
        private bool Compare(Pose a, Pose b)
        {
            return this.Compare(a.Position.X, b.Position.X) && this.Compare(a.Position.Y, b.Position.Y)
                   && this.Compare(a.Position.Z, b.Position.Z) && this.Compare(a.Orientation.X, b.Orientation.X)
                   && this.Compare(a.Orientation.Y, b.Orientation.Y) && this.Compare(a.Orientation.Z, b.Orientation.Z)
                   && this.Compare(a.Orientation.W, b.Orientation.W);
        }
    }
}
